#include "TimerThree.h"
#include <Arduino.h>
#include <Servo.h> 
#include <avr/io.h>
#include <avr/interrupt.h>
#include "config.h"
#include "motors.h"
#include "ADC.h"

int sensorTSOP[3];
int tiksTSOP = 0; 
int leftTSOP=0,
    rightTSOP=0,
    centerTSOP=0;


int sensorTSOPNow = 0; // sensor now analysing
int workToDo = 0;
void TimerThreeInterrupt ()
{
  switch(sensorTSOPNow)
  {
    case 0:
      switch(workToDo)
      {
        case 0: 
          Timer3.pwm(SENSOR_LED_LEFT, 50); // LEFT
          sensorTSOP[SENSOTR_TSOP_LEFT]=0;
          workToDo = 1;
          tiksTSOP=0;
        break;
        case 1:
          tiksTSOP++;
          if(tiksTSOP>30)
          {
            workToDo = 0;
            sensorTSOPNow = 1;
            leftTSOP=sensorTSOP[SENSOTR_TSOP_LEFT];
            Timer3.disablePwm(SENSOR_LED_LEFT);
          }
          else
          {
            sensorTSOP[SENSOTR_TSOP_LEFT]+=digitalRead(SENSOR_DIGITAL_LEFT);
          }
          
      }
    break;
    case 1:
      switch(workToDo)
      {
        case 0: 
          Timer3.pwm(SENSOR_LED_CENTER, 130); // center
          sensorTSOP[SENSOTR_TSOP_CENTER]=0;
          workToDo = 1;
          tiksTSOP=0;
        break;
        case 1:
          tiksTSOP++;
          if(tiksTSOP>30)
          {
            workToDo = 0;
            sensorTSOPNow = 2;
            centerTSOP=sensorTSOP[SENSOTR_TSOP_CENTER];
            Timer3.disablePwm(SENSOR_LED_CENTER);
          }
          else
          {
            sensorTSOP[SENSOTR_TSOP_CENTER]+=digitalRead(SENSOR_DIGITAL_CENTER);
          }
          
      }
    break;
    case 2:
       switch(workToDo)
      {
        case 0: 
          Timer3.pwm(SENSOR_LED_RIGHT, 10); //right
          sensorTSOP[SENSOTR_TSOP_RIGHT]=0;
          workToDo = 1;
          tiksTSOP=0;
        break;
        case 1:
          tiksTSOP++;
          if(tiksTSOP>30)
          {
            workToDo = 0;
            sensorTSOPNow = 0;
            rightTSOP=sensorTSOP[SENSOTR_TSOP_RIGHT];
            Timer3.disablePwm(SENSOR_LED_RIGHT);
          }
          else
          {
            sensorTSOP[SENSOTR_TSOP_RIGHT]+=digitalRead(SENSOR_DIGITAL_RIGHT);
          }
          
      }
    break;
  } 
  
  
}

void TimerThreeInit()
{
  Timer3.initialize(26);         // initialize timer 3    27 !!!!!
  Timer3.attachInterrupt(TimerThreeInterrupt);
}

